#ifndef IMU_FACTOR_H
#define IMU_FACTOR_H
#include <iostream>
#include <vector>
#include <memory>

#include <ceres/ceres.h>

#include "imu_preintegration.h"

#include "state.h"

class ImuFactor : public ceres::SizedCostFunction<5, 8, 5>{
public:  
   ImuFactor(std::shared_ptr<ImuPreIntegration> imu_preintegration);

   Eigen::Matrix<double, 5, 5> GetCov() {
      return imu_preintegration_->cov_;
   }
   // ceres-solver
   // parameters[0] : pi, theta_i, vi, bg, ba;
   // parameters[1] : pj, theta_j, vj;
   virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
      double Ri = parameters[0][0];
      Eigen::Vector2d pi(parameters[0][1], parameters[0][2]);
      Eigen::Vector2d vi(parameters[0][3], parameters[0][4]);
      double bg = parameters[0][5];
      Eigen::Vector2d ba(parameters[0][6], parameters[0][7]);
      double Rj = parameters[1][0];
      Eigen::Vector2d pj(parameters[1][1], parameters[1][2]);
      Eigen::Vector2d vj(parameters[1][3], parameters[1][4]);

      Eigen::Map<Eigen::Matrix<double, 5, 1>> residual(residuals);

      double dt = imu_preintegration_->dt_;

      SO2 dR = imu_preintegration_->GetDeltaRotation(bg);
      Eigen::Vector2d dv = imu_preintegration_->GetDeltaVelocity(bg, ba);
      Eigen::Vector2d dp = imu_preintegration_->GetDeltaPosition(bg, ba);

      Eigen::Matrix2d Rit = SO2(Ri).inverse(); 

      //（九）(30)
      // residual(0,0) = SO2::exp(dR.inverse() * SO2(Ri).inverse() * SO2(Rj).matrix());

      // //（九）(31)
      // residual.block<2,1>(1,0) = Rit * (vj - vi) - dv;

      // //（九）(32)
      // residual.block<2,1>(3,0) = Rit * (pj - pi - vi * dt) - dp;

      //（九）(30)
      residual.block<2,1>(0,0) = Rit * (pj - pi - vi * dt) - dp;
      //（九）(32)
      residual(2,0) = SO2::exp(dR.inverse() * SO2(Ri).inverse() * SO2(Rj).matrix());
      //（九）(31)
      residual.block<2,1>(3,0) = Rit * (vj - vi) - dv;

      Eigen::Matrix<double, 5, 5> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 5, 5>>(imu_preintegration_->cov_.inverse()).matrixL().transpose();
      residual = sqrt_info * residual;

      //   col=0   col=1,2 col=3,4 col=5   col=6,7 col=8   col=9,10 col=11,12
      // | dR1/dR1 dR1/dp1 dR1/dv1 dR1/dbg dR1/dba dR1/dR2 dR1/dp2 dR1/dv2|
      // | dv1/dR1 dv1/dp1 dv1/dv1 dv1/dbg dv1/dba dv1/dR2 dv1/dp2 dv1/dv2|
      // | dp1/dR1 dp1/dp1 dp1/dv1 dp1/dbg dp1/dba dp1/dR2 dp1/dp2 dp1/dv2|
      // | dR2/dR1 dR2/dp1 dR2/dv1 dR2/dbg dR2/dba dR2/dR2 dR2/dp2 dR2/dv2|
      // | dv2/dR1 dv2/dp1 dv2/dv1 dv2/dbg dv2/dba dv2/dR2 dv2/dp2 dv2/dv2|
      // | dp2/dR1 dp2/dp1 dp2/dv1 dp2/dbg dp2/dba dp2/dR2 dp2/dp2 dp2/dv2|
      if (jacobians) {
         if (jacobians[0]) {
            Eigen::Map<Eigen::Matrix<double, 5, 8, Eigen::RowMajor>> jacobianXi(jacobians[0]);
            jacobianXi.setZero();

            // compute jacobian, last_state
            // dR/dR1
            jacobianXi(0,0) = -1; // (33)
            // dR/dbg1
            jacobianXi(0,7) = -dt; // (35)

            // dv/dR1
            jacobianXi.block<2,1>(1,0) = -SO2::so2(1) * Rit * (vj - vi); // (38)
            // dv/dv1
            jacobianXi.block<2,2>(1,3) = -Rit; // (36)
            // dv/dba1
            jacobianXi.block<2,2>(1,5) = -imu_preintegration_->dV_dba_; // -(26)
            // dv/dbg1
            jacobianXi.block<2,1>(1,7) = -imu_preintegration_->dV_dbg_; // -(27)

            // dp/dR1
            jacobianXi.block<2,1>(3,0) = -SO2::so2(1) * Rit * (pj - pi - vi * dt); // (42)
            // dp/dp1
            jacobianXi.block<2,2>(3,1) = -Rit; // (39)
            // dp/dv1
            jacobianXi.block<2,2>(3,3) = -Rit * dt; // (41)
            // dp/dba1
            jacobianXi.block<2,2>(3,5) = -imu_preintegration_->dP_dba_;
            // dp/dbg1
            jacobianXi.block<2,1>(3,7) = -imu_preintegration_->dP_dbg_;

         }

         if (jacobians[1]) {
            Eigen::Map<Eigen::Matrix<double, 5, 5, Eigen::RowMajor>> jacobianXj(jacobians[1]);
            jacobianXj.setZero();
            // dR/dR2
            jacobianXj(0,0) = 1; // (34)
            // dv/dv2
            jacobianXj.block<2,2>(1,3) = Rit; // (37)
            // dp/dv2
            jacobianXj.block<2,2>(3,1) = Rit; // (40)
         }

      }
   }

private:
   std::shared_ptr<ImuPreIntegration> imu_preintegration_;
};

#endif 
